#ifndef CONTROLLER_H
#define CONTROLLER_H

#include <stdint.h>
#include <stdlib.h> 
#include <string.h> 
#include "math.h"
#include "bsp_dwt.h"

#ifndef abs
#define abs(x) ((x) > 0 ? (x) : -(x))
#endif

typedef enum
{
    PID_IMPROVE_NONE                = 0b00000000,
    PID_Integral_Limit              = 0b00000001,
    PID_Derivative_On_Measurement   = 0b00000010,
    PID_Trapezoid_Intergral         = 0b00000100,
    PID_Proportional_On_Measurement = 0b00001000,
    PID_OutputFilter                = 0x00010000,
    PID_ChangingIntegrationRate     = 0x00100000,
    PID_DerivativeFilter            = 0x01000000,
    PID_ErrorHandle                 = 0x10000000,
} PID_Improvement_e;

typedef enum errorType_e
{
    PID_ERROR_NONE = 0x00U,
    Motor_Blocked = 0x01U
} ErrorType_e;

typedef struct
{
    uint64_t ERRORCount;
    ErrorType_e ERRORType;
} PID_ErrorHandler_t;

typedef struct
{
    float Kp;
    float Ki;
    float Kd;
    float MaxOut;
    float DeadBand;
    
    PID_Improvement_e Improve;
    float IntegralLimit;
    float CoefA;         //For Changing Integral
    float CoefB;         //ITerm = Err*((A-abs(err)+B)/A)  when B<|err|<A+B
    float Output_LPF_RC; // RC = 1/omegac
    float Derivative_LPF_RC;

    //下面这些是用来计算的
    float Measure;
    float Last_Measure;
    float Err;
    float Last_Err;
    float Last_ITerm;

    float Pout;
    float Iout;
    float Dout;
    float ITerm;

    float Output;
    float Last_Output;
    float Last_Dout;

    float Ref;
    uint32_t DWT_CNT;
    float dt;

    PID_ErrorHandler_t ERRORHandler;
} PIDInstance;

typedef struct
{
    // basic parameter
    float Kp;
    float Ki;
    float Kd;
    float MaxOut;   // 输出限幅
    float DeadBand; // 死区
    
    // improve parameter
    PID_Improvement_e Improve;
    float IntegralLimit; // 积分限幅
    float CoefA;         // AB为变速积分参数,变速积分实际上就引入了积分分离
    float CoefB;         // ITerm = Err*((A-abs(err)+B)/A)  when B<|err|<A+B
    float Output_LPF_RC; // RC = 1/omegac
    float Derivative_LPF_RC;
} PID_Init_Config_s;

void PID_Init(PIDInstance* pid, PID_Init_Config_s* config);
float PID_Calc(PIDInstance* pid, float ref, float measure);
#endif
